#include "sj_pid.h"
#include <math.h>
// 常规PID
float sf_pid_solve(sf_pid_param_t *pid, float error) {
    pid->out_d = (error - pid->out_p) * pid->low_pass + pid->out_d * (1 - pid->low_pass);
    pid->out_p = error;
    pid->out_i += error;

    if (pid->ki != 0) pid->out_i = SF_MINMAX(pid->out_i, -pid->i_max / pid->ki, pid->i_max / pid->ki);

    return pid->kp * pid->out_p + pid->ki * pid->out_i + pid->kd * pid->out_d;
}

// 增量式PID
float sf_increment_pid_solve(sf_pid_param_t *pid, float error) {
    pid->out_d = SF_MINMAX(pid->kd * (error - 2 * pid->pre_error + pid->pre_pre_error), -pid->d_max, pid->d_max);
    pid->out_p = SF_MINMAX(pid->kp * (error - pid->pre_error), -pid->p_max, pid->p_max);
    pid->out_i = SF_MINMAX(pid->ki * error, -pid->i_max, pid->i_max);

    pid->pre_pre_error = pid->pre_error;
    pid->pre_error = error;

    return pid->out_p + pid->out_i + pid->out_d;
}


float sf_change_kib = 4;

//变积分PID，e大i小
float sf_changable_pid_solve(sf_pid_param_t *pid, float error) {
    pid->out_d = pid->kd * (error - 2 * pid->pre_error + pid->pre_pre_error);
    pid->out_p = pid->kp * (error - pid->pre_error);
    float ki_index = pid->ki;
    if (error + pid->pre_error > 0) {
        ki_index = (pid->ki) - (pid->ki) / (1 + expf(sf_change_kib - 0.2f * fabsf(error)));    //变积分控制
    }

    pid->out_i = ki_index * error;
    pid->pre_pre_error = pid->pre_error;
    pid->pre_error = error;

    return SF_MINMAX(pid->out_p, -pid->p_max, pid->p_max)
           + SF_MINMAX(pid->out_i, -pid->i_max, pid->i_max)
           + SF_MINMAX(pid->out_d, -pid->d_max, pid->d_max);
}

float sf_bangbang_out = 0;

float sf_bangbang_pid_solve(sf_pid_param_t *pid, float error) {
    float BangBang_output = 15000, BangBang_error = 8;
    pid->error = error;

    //BangBang
    if (error > BangBang_error || error < -BangBang_error) {
        sf_bangbang_out = (error > 0) ? BangBang_output : (-BangBang_output);
    } else {
        pid->out_d = pid->kd * (error - 2 * pid->pre_error + pid->pre_pre_error);

        pid->out_p = pid->kp * (error - pid->pre_error);

        pid->out_i = pid->ki * error;

        sf_bangbang_out = SF_MINMAX(pid->out_p, -pid->p_max, pid->p_max)
                       + SF_MINMAX(pid->out_i, -pid->i_max, pid->i_max)
                       + SF_MINMAX(pid->out_d, -pid->d_max, pid->d_max);

    }
    pid->pre_pre_error = pid->pre_error;
    pid->pre_error = error;

    return sf_bangbang_out;
}

// ====================== 在文件末尾添加以下新函数 ======================

/**
 * @brief 非线性PID求解器 (专为舵机优化)
 * P项与误差的平方成正比，I项和D项保持不变
 * @param pid PID参数结构体指针
 * @param error 输入的误差 (例如 pure_angle)
 * @return 计算出的控制量 (例如最终的舵机角度)
 */
float sf_nonlinear_pid_solve(sf_pid_param_t *pid, float error) {
    // --- D项 (微分) ---
    // D项保持不变，用于抑制抖动，保持稳定性
    pid->out_d = (error - pid->out_p) * pid->low_pass + pid->out_d * (1 - pid->low_pass);

    // --- P项 (比例) ---
    // 【【【 核心修改：将线性P项替换为二次函数P项 】】】
    // 原始P项: pid->out_p = error;
    // 新的P项: 我们直接计算最终的P输出
    float p_term_out;
    if (error >= 0) {
        // 使用 error * error 来实现二次关系
        p_term_out = pid->kp * error * error; 
    } else {
        // 为了保留负号，使用 -error * error
        p_term_out = -pid->kp * error * error;
    }

    // --- I项 (积分) ---
    // I项保持不变，对于舵机通常 Ki=0，所以 out_i 也为 0
    pid->out_i += error;
    if (pid->ki != 0) {
        pid->out_i = SF_MINMAX(pid->out_i, -pid->i_max / pid->ki, pid->i_max / pid->ki);
    }
    
    // 记录当前误差，供下一轮D项计算使用
    pid->out_p = error;

    // 返回最终控制量
    // 注意：这里的 p_term_out 已经是乘以 Kp 之后的结果了
    return p_term_out + pid->ki * pid->out_i + pid->kd * pid->out_d;
}

// ====================== 添加结束 ======================
